1 /*
2 * File: pitch_ap.c
3 *
4 * Code generated for Simulink model 'pitch_ap'.
5 *
6 * Model version : 1.90
7 * Simulink Coder version : 8.5 (R2013b) 08-Aug-2013
8 * C/C++ source code generated on : Mon Feb 03 08:13:44 2014
9 *
10 * Target selection: ert.tlc
11 * Embedded hardware selection: 32-bit Embedded Processor
12 * Code generation objectives: Unspecified
13 * Validation result: Not run
14 */
15
16 #include "pitch_ap.h"
17 #include "pitch_ap_private.h"
18
19 /* Start for referenced model: 'pitch_ap' */
20 void pitch_ap_Start(rtDW_pitch_ap *localDW)
21 {
22 /* Start for ModelReference: '<Root>/Altitude Mode' */
23 Altitude_Mode_Start(&(localDW->AltitudeMode_DWORK1.rtb));
24 }
25
26 /* Output and update for referenced model: 'pitch_ap' */
27 void pitch_ap(const real_T *rtu_Phi, const real_T *rtu_Theta, const real_T
28 *rtu_Q, const real_T *rtu_R, const real_T *rtu_Alt, const real_T
29 *rtu_AltRate, const real_T *rtu_TAS, const boolean_T *rtu_APEng,
30 const boolean_T *rtu_ALTMode, const real_T *rtu_AltRef, const
31 real_T *rtu_PitchWheel, real_T *rty_ElvCmd, rtB_pitch_ap *localB,
32 rtDW_pitch_ap *localDW, real_T rtp_dispGain, real_T rtp_intGain,
33 real_T rtp_rateGain)
34 {
35 /* local block i/o variables */
36 real_T rtb_AltitudeMode;
37 real_T rtb_BasicPitchMode;
38 real_T rtb_Sum3_pjva;
39 real_T rtb_Sum3;
40 real_T rtb_UnitConversion;
41 real_T rtb_Xnew;
42
43 /* ModelReference: '<Root>/Altitude Mode' */
44 Altitude_Mode(rtu_AltRef, rtu_Alt, rtu_AltRate, rtu_TAS, rtu_Theta,
45 rtu_ALTMode, &rtb_AltitudeMode,
46 &(localDW->AltitudeMode_DWORK1.rtb),
47 &(localDW->AltitudeMode_DWORK1.rtdw));
48
49 /* Outputs for Atomic SubSystem: '<Root>/Pitch Reference' */
50 /* Switch: '<S5>/Enable' incorporates:
51 * Logic: '<S3>/Not engaged'
52 * UnitDelay: '<S5>/FixPt Unit Delay1'
53 */
54 if (!(*rtu_APEng)) {
55 rtb_Xnew = *rtu_Theta;
56 } else {
57 rtb_Xnew = localDW->FixPtUnitDelay1_DSTATE;
58 }
59
60 /* End of Switch: '<S5>/Enable' */
61
62 /* Sum: '<S3>/Sum3' incorporates:
63 * UnitDelay: '<S5>/FixPt Unit Delay1'
64 */
65 rtb_Sum3 = localDW->FixPtUnitDelay1_DSTATE + (*rtu_PitchWheel);
66
67 /* Update for UnitDelay: '<S5>/FixPt Unit Delay1' */
68 localDW->FixPtUnitDelay1_DSTATE = rtb_Xnew;
69
70 /* End of Outputs for SubSystem: '<Root>/Pitch Reference' */
71
72 /* Switch: '<Root>/Mode switch' */
73 if (*rtu_ALTMode) {
74 localB->thetaCmd = rtb_AltitudeMode;
75 } else {
76 localB->thetaCmd = rtb_Sum3;
77 }
78
79 /* End of Switch: '<Root>/Mode switch' */
80
81 /* Outputs for Atomic SubSystem: '<Root>/Pitch Rate Feedback' */
82 /* Gain: '<S4>/Unit Conversion' */
83 rtb_UnitConversion = 0.017453292519943295 * (*rtu_Phi);
84
85 /* Sum: '<S2>/Sum3' incorporates:
86 * Product: '<S2>/Product'
87 * Product: '<S2>/Product1'
88 * Trigonometry: '<S2>/cos'
89 * Trigonometry: '<S2>/sin'
90 */
91 rtb_Sum3_pjva = (cos(rtb_UnitConversion) * (*rtu_Q)) + (sin(rtb_UnitConversion)
92 * (*rtu_R));
93
94 /* End of Outputs for SubSystem: '<Root>/Pitch Rate Feedback' */
95
96 /* ModelReference: '<Root>/Basic Pitch Mode' */
97 attitude_controller(&localB->thetaCmd, rtu_Theta, &rtb_Sum3_pjva, rtu_APEng,
98 &rtb_BasicPitchMode, &(localDW->BasicPitchMode_DWORK1.rtdw),
99 rtp_dispGain, 20.0, rtp_rateGain, 3.0, rtp_intGain, 5.0,
100 20.0);
101
102 /* Switch: '<Root>/Eng switch' incorporates:
103 * Constant: '<Root>/Constant'
104 */
105 if (*rtu_APEng) {
106 *rty_ElvCmd = rtb_BasicPitchMode;
107 } else {
108 *rty_ElvCmd = 0.0;
109 }
110
111 /* End of Switch: '<Root>/Eng switch' */
112 }
113
114 /* Model initialize function */
115 void pitch_ap_initialize(rtB_pitch_ap *localB, rtDW_pitch_ap *localDW)
116 {
117 /* Registration code */
118
119 /* block I/O */
120 (void) memset(((void *) localB), 0,
121 sizeof(rtB_pitch_ap));
122
123 /* states (dwork) */
124 (void) memset((void *)localDW, 0,
125 sizeof(rtDW_pitch_ap));
126
127 /* Model Initialize fcn for ModelReference Block: '<Root>/Altitude Mode' */
128 Altitude_Mode_initialize(&(localDW->AltitudeMode_DWORK1.rtb),
129 &(localDW->AltitudeMode_DWORK1.rtdw));
130
131 /* Model Initialize fcn for ModelReference Block: '<Root>/Basic Pitch Mode' */
132 attitude_controller_initialize(&(localDW->BasicPitchMode_DWORK1.rtdw));
133 }
134
135 /*
136 * File trailer for generated code.
137 *
138 * [EOF]
139 */
140
|